package roborally.pathfinding;

import java.util.HashMap;
import java.util.Map;

import be.kuleuven.cs.som.annotate.*;

import roborally.exceptions.InvalidPositionException;
import roborally.model.EnergyAmount;
import roborally.model.Orientation;
import roborally.model.Position;
import roborally.model.Robot;

/**
 * @author  Pieter Robberechts	(1e Bach Informatica)
 * @author	Jeroen Sanders		(1e Bach Informatica)
 */
public class PathFinding {
	
	/**
	 * Initializes a new path from the robot towards the given position.
	 * @param	robot
	 * 			The robot for which the path will be calculated.
	 * @param	end
	 * 			The destination for the path.
	 */
	public PathFinding(Robot robot,Position end){
		this.robot = robot;
		this.end = end;
		this.start = new Node(null,robot.getPosition(),0,calculatehCost(robot.getPosition()));
		this.open.put(robot.getPosition(),start);
		this.calculatePath();
	}
	
	/**
	 * Initializes a new path from the first robot towards a position next
	 * to the second robot.
	 * @param	robot
	 * 			The first robot for which the path will be calculated.
	 * @param	destination
	 * 			The second robot which will be the reference for the destination.
	 */
	public PathFinding(Robot robot, Robot destination) {
		this.robot = robot;
		this.end = destination.getPosition();
		this.start = new Node(null,robot.getPosition(),0,calculatehCost(robot.getPosition()));
		this.open.put(robot.getPosition(),start);
		this.toRobot = true;
		this.calculatePath();
	}
	
	/**
	 * Private constructor used for reversing a path.
	 */
	private PathFinding(Robot robot,Node previousEnd,Position previousStart) {
		this.robot = robot;
		this.end = previousStart;
		this.start = new Node(null,previousEnd.getPosition(),0,calculatehCost(previousEnd.getPosition()));
		this.closed.put(start.getPosition(),start);
		Node parent = start;
		Node nextNode = previousEnd.getParent();
		while(!nextNode.getPosition().equals(end)) {
			this.closed.put(nextNode.getPosition(),
					new Node(parent,nextNode.getPosition(),
					calculategCost(parent, nextNode.getPosition()),
					calculatehCost(nextNode.getPosition())));
			parent = closed.get(nextNode.getPosition());
			nextNode = nextNode.getParent();
		}
		this.closed.put(end, new Node(parent,end,
				calculategCost(parent, end),0));
		this.toRobot = true;
	}
	
	/**
	 * Holds the robot for which the path is calculated.
	 */
	private final Robot robot;
	
	/**
	 * Returns the energy required for this robot to move.
	 */
	private double getMoveEnergy(){
		return robot.getEnergyRequiredToMove().getAmountInUnits();
	}
	
	/**
	 * Returns the energy required for this robot to turn.
	 */
	private double getTurnEnergy(){
		return Robot.getEnergyRequiredToTurn().getAmountInUnits();
	}
	
	/**
	 * Holds whether the path has another robot as destination.
	 */
	private boolean toRobot = false;
	
	/**
	 * Holds the starting-node of the path.
	 */
	private final Node start;

	/**
	 * Holds the destination of the path.
	 */
	private final Position end;
	
	/**
	 * Returns the destination of the path.
	 */
	@Basic @Immutable
	public Position getEnd(){
		return end;
	}
	
	/**
	 * Hashmap containing all the open nodes.
	 */
	private Map<Position,Node> open = new HashMap<Position,Node>();
	
	/**
	 * Hashmap containing all the closed nodes.
	 */
	private Map<Position,Node> closed = new HashMap<Position,Node>();
	
	/**
	 * Returns the list containing all the closed nodes.
	 */
	@Basic
	public Map<Position,Node> getClosedList() {
		return closed;
	}
	
	/**
	 * Calculates the heuristic cost of the given position to the end.
	 */
	private double calculatehCost(Position pos) {
		return pos.getManhattanDistance(end)*getMoveEnergy();
	}
	
	/**
	 * Calculates the cost from start to the given position.
	 */
	private double calculategCost(Node parent,Position pos) {
		try {
		if(parent.getPosition().getNeighbour(getOrientation(parent)).equals(pos))
			return parent.getgCost() + getMoveEnergy();
		} catch(InvalidPositionException e){}
		try {
			if( parent.getPosition().getNeighbour(getOrientation(parent).turn90Clockwise()).equals(pos))
				return parent.getgCost() + getMoveEnergy() + getTurnEnergy();
		} catch(InvalidPositionException e) {}
		try {
			if( parent.getPosition().getNeighbour(getOrientation(parent).turn90CounterClockwise()).equals(pos))
				return parent.getgCost() + getMoveEnergy() + getTurnEnergy();
		} catch(InvalidPositionException e) {}
		return parent.getgCost() + getMoveEnergy() + 2*getTurnEnergy();
	}
	
	/**
	 * Returns a node from the open-list with the lowest f-score.
	 */
	private Node getLowestfScore() {
		Node lowest = null;
		for(Position pos : open.keySet()) {
			if(lowest == null || open.get(pos).getfCost() <= lowest.getfCost()) {
				lowest = open.get(pos);
			}
		}
		return lowest;
	}
	
	/**
	 * Returns whether this robot can have the given position as a position.
	 */
	private boolean isWalkable(Position pos) {
		if(toRobot && pos.equals(end)) {
				return true;
		}
		if(pos.getX() > robot.getPosition().getX() + 81 ||
		   pos.getX() < robot.getPosition().getX() - 81 ||
		   pos.getY() > robot.getPosition().getY() + 81 ||
		   pos.getY() < robot.getPosition().getY() - 81)
			return false;
		try {
			return robot.getBoard().canHaveElementOnPosition(robot,pos);
		} catch (Exception e) {
			return false;
		}
	}

	/**
	 * Returns the orientation the robot would have on the given node.
	 * 
	 * @param	node
	 * 			The node for which the orientation must be returned.
	 * 
	 * @return	...
	 * 			| if(node.getParent() == null)
	 *			|	then result ==  robot.getOrientation();
	 *			| if(node.getParent().getPosition().getY()-1 == node.getPosition().getY())
	 *			|	then result == Orientation.UP;
	 *			| if(node.getParent().getPosition().getY()+1 == node.getPosition().getY())
	 *			|	then result == Orientation.DOWN;
	 *			| if(node.getParent().getPosition().getX()-1 == node.getPosition().getX())
	 *			|	then result == Orientation.LEFT;
	 *			| if(node.getParent().getPosition().getX()+1 == node.getPosition().getX())
	 *			|	then result == Orientation.RIGHT;
	 */
	@Model
	private Orientation getOrientation(Node node) {
		Node parent = node.getParent();
		if(parent == null)
			return robot.getOrientation();
		if(parent.getPosition().getY()-1 == node.getPosition().getY())
			return Orientation.UP;
		if(parent.getPosition().getY()+1 == node.getPosition().getY())
			return Orientation.DOWN;
		if(parent.getPosition().getX()-1 == node.getPosition().getX())
			return Orientation.LEFT;
		if(parent.getPosition().getX()+1 == node.getPosition().getX())
			return Orientation.RIGHT;
		return null;
	}
	
	/**
	 * Subroutine of calculatePath() which performs some checks and calculations
	 * on a neighbour of the given node.
	 */
	private void checkNeighbour(Node node,Orientation orientation) {
		Position next = null;
		try{
		next = node.getPosition().getNeighbour(orientation);
		} catch (InvalidPositionException e){return;}
		try {
			if(isWalkable(next) && !closed.containsKey(next)) {
				if(!open.containsKey(next)) {
					Node nextNode = new Node(node, next, calculategCost(node, next), calculatehCost(next));
					open.put(next,nextNode);
				} else {
					if(open.get(next).getgCost() > calculategCost(node,next)) {
						open.get(next).setParent(node);
						open.get(next).setgCost(calculategCost(node,next));
					}
				}
			}
		} catch (Exception e) {
			//Catch IllegalArgumentException and InvalidPositionException
		}
		
	}
	
	/**
	 * Routine which tries to calculate a path towards the destination.
	 */
	private void calculatePath(){
		Node current;
		while(!open.isEmpty() && !pathFound()){
			current = getLowestfScore();
			closed.put(current.getPosition(), current);
			open.remove(current.getPosition());
			checkNeighbour(current, Orientation.UP);
			checkNeighbour(current, Orientation.RIGHT);
			checkNeighbour(current, Orientation.DOWN);
			checkNeighbour(current, Orientation.LEFT);
		}
	}
	
	/**
	 * Returns the minimal energy required for the robot to reach
	 * the given destination.
	 * 
	 * @param	position
	 * 			The destination for which the minimal energy will
	 * 			be returned.
	 * @return	...
	 * 			| result == getClosedList().get(position).getgCost()
	 */
	public EnergyAmount getEnergyRequired(Position position) {
		if(closed.containsKey(position)) {
			if(toRobot && end == position && closed.get(position).getParent() != null) {
				return new EnergyAmount(closed.get(position).getParent().getgCost());	
			}
			return new EnergyAmount(closed.get(position).getgCost());
		}
		return null;
	}
	
	/**
	 * If the path has been calculated, this returns the minimal
	 * energy required for the robot to reach the destination.
	 * 
	 * @return	...
	 * 			| result == getClosedList().get(getEnd()).getgCost()
	 */
	public EnergyAmount getEnergyRequired() {
		return getEnergyRequired(end);
	}
	
	/**
	 * Returns whether the shortest path towards the destination has been found.
	 * @return 	...
	 * 			| result == getClosedList().containsKey(getEnd())
	 */
	public boolean pathFound() {
		return closed.containsKey(end);
	}
	
	/**
	 * Returns the previous position in the path.
	 * @param	position
	 * 			The position after the position required to return.
	 * @return	...
	 * 			| if (getClosedList().get(position).getParent() != null)
	 * 			|	then result == getClosedList().get(position).getParent().getPosition()
	 * 			| else
	 * 			| 	then result == null
	 */
	public Position getPrevious(Position position) {
			if (closed.get(position).getParent() == null)
				return null;
			return closed.get(position).getParent().getPosition();
	}
		
	/**
	 * Returns the closest position on the path for the given energy.
	 * 
	 * @param	energy
	 * 			The energy that limits the robot's movements.
	 * @return	...
	 * 			| if !pathFound()
	 * 			|	then result == null
	 * 			| else
	 * 			|	for one NodeX in path, for each NodeY in path:
	 * 			|		if (NodeY.getgCost() <= energy.getAmountInUnits() && NodeX.getgCost() <= energy.getAmountInUnits()
	 * 			|			&& NodeY.gethCost() >= NodeX.gethCost())
	 * 			|			then result == NodeX.getPosition()
	 * 
	 * @note	The closest position on the path may not be the best one
	 * 			in terms of manhattan distance, use getClosest() instead.
	 */
	public Position getClosestOnPath(EnergyAmount energy) {
		if(!pathFound())
			return null;
		Node current = closed.get(end);
		if(toRobot)
			current = current.getParent();
		while(current.getgCost() > energy.getAmountInUnits()) {
			current = current.getParent();
			if(current == null)
				return null;
		}
		return current.getPosition();
	}
	
	/**
	 * Returns the closest position (in manhattan distance) for the given energy.
	 * 
	 * @param	energy
	 * 			The energy that limits the robot's movements.
	 * @return	...
	 * 			| for one NodeX in getClosedList(), for each NodeY in getClosedList():
	 * 			|	if (NodeY.getgCost() <= energy.getAmountInUnits() && NodeX.getgCost() <= energy.getAmountInUnits()
	 * 			|		&& NodeY.gethCost() >= NodeX.gethCost())
	 * 			|		then result == NodeX.getPosition()
	 */
	public Position getClosest(EnergyAmount energy) {
		Node lowest = null;
		for(Position pos : closed.keySet()) {
			if((lowest == null || closed.get(pos).gethCost() <= lowest.gethCost())
				&& this.closed.get(pos).getgCost() <= energy.getAmountInUnits()) {
				lowest = closed.get(pos);
			}
		}
		return lowest.getPosition();
	}
	
	/**
	 * Returns the orientation the robot would have in the given position
	 * according to the calculated path.
	 * 
	 * @param	position
	 * 			The position for which the orientation will be returned.
	 * @return	...
	 * 			| result == getOrientattion(getClosedList().get(position))
	 */
	public Orientation getOrientation(Position position) {
		return getOrientation(closed.get(position));
	}
		
	/**
	 * Returns a new object of PathFinding with the path reversed for another robot.
	 * 
	 * @param	robot
	 * 			The robot for which the reversed path will be calculated.
	 * 
	 * @return	...
	 * 			| if(!pathFound())
	 * 			|	then result == null
	 * 			| if(!robot.getPosition().equals(getClosedList().get(getEnd())))
	 * 			|	then result == null
	 * 			| else
	 * 			|	result == new PathFinding()
	 * 
	 * @note	Only works if the given robot stands on the destination of this path.
	 * 			If not the case this will return NULL.
	 */
	public PathFinding reverse(Robot robot) {
		if(!pathFound())
			return null;
		if (!robot.getPosition().equals(closed.get(end).getPosition()))
			return null;
		return new PathFinding(robot,closed.get(end),start.getPosition());
	}
		
}